//
//  canny.c
//  FastPRDemo
//
//  Created by fanwenjie on 2017/6/1.
//  Copyright © 2017年 fanwenjie. All rights reserved.
//

#include "canny.h"
#include <memory.h>
#include <math.h>
#include <stdlib.h>
static void Sobel(const unsigned char *src, short *dx, short *dy, const int rows, const int cols)
{
    memset(dx, 0, sizeof(short)*rows*cols);
    memset(dy, 0, sizeof(short)*rows*cols);
    const int LT = -cols - 1, TOP = -cols, RT = -cols + 1, RIGHT = 1, RB = cols + 1, BOTTOM = cols, LB = cols - 1, LEFT = -1;
    dx += RB;
    dy += RB;
    src += RB;
    for (int i = 0; i < rows - 2; ++i)
    {
        for (int j = 0; j < cols - 2; ++j)
        {
            *dx++ = ((src[RIGHT] - src[LEFT]) << 1) + src[RT] - src[LT] + src[RB] - src[LB];
            *dy++ = ((src[BOTTOM] - src[TOP]) << 1) + src[LB] - src[LT] + src[RB] - src[RT];
            ++src;
        }
        dx+=2;
        dy+=2;
        src+=2;
    }
}

static void GassianBlur(const unsigned char *src, unsigned char *dst, const int rows, const int cols, const double sigma)
{
    const int LT = -cols - 1, TOP = -cols, RT = -cols + 1, RIGHT = 1, RB = cols + 1, BOTTOM = cols, LB = cols - 1, LEFT = -1;
    const int n = rows * cols;
    memcpy(dst, src, cols);
    memcpy(dst + (rows - 1)*cols, src + (rows - 1)*cols, cols);
    for (int i = cols; i < n; i += cols)
    {
        dst[i] = src[i];
        dst[i - 1] = src[i - 1];
    }
#define SHIFT 16
    double g1 = exp(-0.5 / (sigma * sigma)), g2 = g1 * g1;
    double k = 1.0 / (1 + 4 * (g1 + g2));
    g1 *= k;
    g2 *= k;
    int gi1 = (int)(g1 *(1 << SHIFT)), gi2 = (int)(g2 *(1 << SHIFT)), gi0 = (1 << SHIFT) - 4 * (gi1 + gi2);
    dst += RB;
    src += RB;
    for (int i = 0; i < rows - 2; ++i)
    {
        for (int j = 0; j < cols - 2; ++j)
        {
            *dst++ = (gi2 * (src[LB] + src[RB] + src[LT] + src[RT]) + gi1 * (src[TOP] + src[BOTTOM] + src[LEFT] + src[RIGHT]) + gi0 *src[0] + (1 << (SHIFT - 1))) >> SHIFT;
            ++src;
        }
        dst+=2;
        src+=2;
    }
#undef SHIFT
}

void Canny(unsigned char *src, unsigned char *dst, const int cols, const int rows, const int low_thresh, const int high_thresh)
{
    const int n = cols * rows;
    int low = low_thresh * low_thresh;
    int high = high_thresh * high_thresh;
    if (low > high)
    {
        low ^= high;
        high ^= low;
        low ^= high;
    }
    
    void *buffer = malloc(n*(sizeof(char) + 2 * sizeof(short) + sizeof(int) + sizeof(char *)));
    unsigned char *map = (unsigned char *)buffer;
    short *dx = (short *)(map + n);
    short *dy = dx + n;
    int *normal = (int *)(dy + n);
    unsigned char **stack = (unsigned char **)(normal + n);
    unsigned char **top = stack;
    GassianBlur(src, src, rows, cols, 0.5);
    Sobel(src, dx, dy, rows, cols);
    for (int i = 0; i < n; ++i)
        normal[i] = dx[i] * dx[i] + dy[i] * dy[i];
#define INNER 0
#define UNCERTAIN 1
#define EDGE 2
    memset(map, INNER, n);
    const int LEFT = -1, TOP = -cols, RIGHT = 1, BOTTOM = cols;
    for (int i = 2; i < rows - 2; i++)
    {
        for (int j = 2; j < cols - 2; j++)
        {
            int k = i * cols + j;
            int *m = normal + k;
            if (*m > low)
            {
                int x = dx[k];
                int y = dy[k];
                int t1 = (y - x)*(y + x);
                int t2 = x * y << 1;
                int t3 = RIGHT;
                if (t2 < 0)
                {
                    t2 = -t2;
                    t3 = LEFT;
                }
                
                if (t1 < -t2)
                {
                    if (*m <= m[LEFT] || *m <= m[RIGHT])
                        continue;
                }
                else if (t1 > t2)
                {
                    if (*m <= m[BOTTOM] || *m <= m[TOP])
                        continue;
                }
                else
                {
                    if (*m <= m[BOTTOM - t3] || *m <= m[TOP + t3])
                        continue;
                }
                if (*m > high)
                {
                    *top = map + k;
                    **top++ = EDGE;
                }
                else
                    map[k] = UNCERTAIN;
            }
        }
    }
    
    const int orient[] = { TOP + LEFT,TOP,TOP + RIGHT,RIGHT,BOTTOM + RIGHT,BOTTOM,BOTTOM + LEFT,LEFT };
    while (top > stack)
    {
        unsigned char* m = *--top;
        for (int i = 0; i < sizeof(orient) / sizeof(*orient); ++i)
        {
            int j = orient[i];
            if (m[j] == UNCERTAIN) {
                *top = m + j;
                **top++ = EDGE;
            }
        }
    }
    for (int i = 0; i < n; ++i)
        dst[i] = (unsigned char)-(map[i] == EDGE);
#undef INNER
#undef UNCERTAIN
#undef EDGE
    free(buffer);
}


